/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package kinect_biomec.Logic;

import Testes.SimpleOpenNI;
import processing.core.PApplet;
import processing.core.PMatrix3D;
import processing.core.PVector;

/**
 *
 * @author vanderson
 */
public class KinectView3D extends PApplet {

    SimpleOpenNI context;
    float zoomF = 0.5f;
    float rotX = radians(180);  // by default rotate the hole scene 180deg around the x-axis, 
    // the data from openni comes upside down
    float rotY = radians(0);
    private int skelProfile = 2;
    private boolean calcPontoDeMass3D = false;
    private boolean calcPontoDeMass2D = false;
    private boolean calcPontoDeMassMembros = false;
    private CalculoCentroDeGravidade calc;
    private PVector pontoM;
    public String msg;
    

    public void echo(String msg){
        this.msg=msg;
    }
    public void setup() {
        size(640, 480, P3D);  // strange, get drawing error in the cameraFrustum if i use P3D, in opengl there is no problem
      /*  context = new SimpleOpenNI(this);
        
        // disable mirror
        context.setMirror(false);
        
        // enable depthMap generation 
        if (context.enableDepth() == false) {
        println("Can't open the depthMap, maybe the camera is not connected!");
        exit();
        return;
        }*/

        // enable skeleton generation for all joints
        //context.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);

        stroke(255, 255, 255);
        smooth();
        perspective(radians(45),
                (float) (width) / (float) (height),
                10, 150000);
    }

    public void setContext(SimpleOpenNI context) {
        this.context = context;

        calc = new CalculoCentroDeGravidade(this.context);
    }
    PVector realWorldPoint;
    int[] depthMap;
    int[] userMap;

    public void draw() {
        // update the cam
        if (context != null) {
            try {
                context.update();

                background(255, 255, 255);

                // set the scene pos
                translate(width / 2, height / 2, 0);
                rotateX(rotX);
                rotateY(rotY);
                scale(zoomF);

                //depthMap = context.depthMap();
                userMap = context.getUsersPixels(1);
                int steps = 3;  // to speed up the drawing, draw every third point
                int index;


                translate(0, 0, -1000);  // set the rotation center of the scene 1000 infront of the camera

                stroke(100);
                for (int y = 0; y < context.depthHeight(); y += steps) {
                    for (int x = 0; x < context.depthWidth(); x += steps) {
                        index = x + y * context.depthWidth();
                        if (userMap[index] > 0) {

                            realWorldPoint = context.depthMapRealWorld()[index];
                            point(realWorldPoint.x, realWorldPoint.y, realWorldPoint.z);
                        }
                    }
                }
                /* PVector point1 = new PVector();
                PVector point2 = new PVector();
                
                context.getSceneFloor(point1, point2);
                point(point1.x,point1.y,point1.z);
                point(point2.x,point2.y,point2.z);*/
                //System.out.println(frameRate);
                //text("FPS: " + frameRate, 20, 20);
                //fill(0, 255, 255);

                // draw the skeleton if it's available
                if (context.isTrackingSkeleton(1) && skelProfile != SimpleOpenNI.SKEL_PROFILE_NONE) {
                    drawSkeleton(1);
                }

                // draw the kinect cam
                // context.drawCamFrustum();
            } catch (Exception e) {
            }
        }
    }

    public void drawPointOfMass(int userId) {
        pontoM = calc.getCentroDeGravidade(userId);
        translate(pontoM.x, pontoM.y, pontoM.z);
        sphere(15);
    }

    public void calcAndDrawPointOfMass(int userId) {
        if (calcPontoDeMass3D) {
            drawPointOfMass(userId);
        }
    }

// draw the skeleton with the selected joints
    public void drawSkeleton(int userId) {
        strokeWeight(3);

        if (skelProfile == SimpleOpenNI.SKEL_PROFILE_ALL
                || skelProfile == SimpleOpenNI.SKEL_PROFILE_HEAD_HANDS
                || skelProfile == SimpleOpenNI.SKEL_PROFILE_UPPER) {

            drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

            drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER,
                    SimpleOpenNI.SKEL_LEFT_ELBOW);

            drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW,
                    SimpleOpenNI.SKEL_LEFT_HAND);


            drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER,
                    SimpleOpenNI.SKEL_RIGHT_ELBOW);

            drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW,
                    SimpleOpenNI.SKEL_RIGHT_HAND);

            drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

            if (skelProfile != SimpleOpenNI.SKEL_PROFILE_HEAD_HANDS) {
                drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
                drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);

                drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
                 drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
            }
        }

        if (skelProfile == SimpleOpenNI.SKEL_PROFILE_ALL
                || skelProfile == SimpleOpenNI.SKEL_PROFILE_LOWER) {

            drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP,
                   SimpleOpenNI.SKEL_LEFT_KNEE);
            drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE,
                    SimpleOpenNI.SKEL_LEFT_FOOT);

            drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP,
                    SimpleOpenNI.SKEL_RIGHT_KNEE);

            drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE,
                    SimpleOpenNI.SKEL_RIGHT_FOOT);



        }
        calcAndDrawPointOfMass(userId);
        strokeWeight(1);

    }

    public void drawLimb(int userId, int jointType1, int jointType2) {
        PVector jointPos1 = new PVector();
        PVector jointPos2 = new PVector();
        float confidence;

        // draw the joint position
        confidence = context.getJointPositionSkeleton(userId, jointType1, jointPos1);
        confidence = context.getJointPositionSkeleton(userId, jointType2, jointPos2);

        stroke(255, 0, 0, confidence * 200 + 55);
        line(jointPos1.x, jointPos1.y, jointPos1.z,
                jointPos2.x, jointPos2.y, jointPos2.z);

        drawJointOrientation(userId, jointType1, jointPos1, 50);
    }

    public void drawJointOrientation(int userId, int jointType, PVector pos, float length) {
        // draw the joint orientation  
        PMatrix3D orientation = new PMatrix3D();
        float confidence = context.getJointOrientationSkeleton(userId, jointType, orientation);
        if (confidence < 0.001f) // nothing to draw, orientation data is useless
        {
            return;
        }

        pushMatrix();
        translate(pos.x, pos.y, pos.z);

        // set the local coordsys
        applyMatrix(orientation);

        // coordsys lines are 100mm long
        // x - r
        stroke(255, 0, 0, confidence * 200 + 55);
        line(0, 0, 0,
                length, 0, 0);
        // y - g
        stroke(0, 255, 0, confidence * 200 + 55);
        line(0, 0, 0,
                0, length, 0);
        // z - b    
        stroke(0, 0, 255, confidence * 200 + 55);
        line(0, 0, 0,
                0, 0, length);
        popMatrix();
    }

// -----------------------------------------------------------------
// SimpleOpenNI user events
    // SimpleOpenNI events
    public void onNewUser(int userId) {
        echo("Novo usuario - userId: " + userId+" iniciando detecção de pose");

        context.startPoseDetection("Psi", userId);
    }

    public void onLostUser(int userId) {
        echo("Usuario perdido - userId: " + userId);
    }

    public void onStartCalibration(int userId) {
        echo("Iniciando calibração - userId: " + userId);
    }

    public void onEndCalibration(int userId, boolean successfull) {
        echo("Calibração finalizada- userId: " + userId + ", status: " + successfull);

        if (successfull) {
            echo("  Usuario calibrado !!!");
            context.startTrackingSkeleton(userId);
        } else {
            echo("  Falha ao calibrar usuario !!!");
            //println("  Start pose detection");
            context.startPoseDetection("Psi", userId);
        }
    }

    public void onStartPose(String pose, int userId) {
        echo("Iniciando pose de calibração - userId: " + userId + ", pose: " + pose);
        //println(" stop pose detection");

        context.stopPoseDetection(userId);
        context.requestCalibrationSkeleton(userId, true);

    }

    public void onEndPose(String pose, int userId) {
        echo("Usuario deixou pose - userId: " + userId + ", pose: " + pose);
    }
// -----------------------------------------------------------------
// Keyboard events
    @Override
    public void keyPressed() {
        switch (key) {
            case ' ':
                context.setMirror(!context.mirror());
                break;
        }

        switch (keyCode) {
            case LEFT:
                rotY += 0.1f;
                break;
            case RIGHT:
                // zoom out
                rotY -= 0.1f;
                break;
            case UP:
                if (keyEvent.isShiftDown()) {
                    zoomF += 0.01f;
                } else {
                    rotX += 0.1f;
                }
                break;
            case DOWN:
                if (keyEvent.isShiftDown()) {
                    zoomF -= 0.01f;
                    if (zoomF < 0.01) {
                        zoomF = (float) 0.01;
                    }
                } else {
                    rotX -= 0.1f;
                }
                break;
        }
    }

    public boolean isCalcPontoDeMass2D() {
        return calcPontoDeMass2D;
    }

    public void setCalcPontoDeMass2D(boolean calcPontoDeMass2D) {
        this.calcPontoDeMass2D = calcPontoDeMass2D;
    }

    public boolean isCalcPontoDeMass3D() {
        return calcPontoDeMass3D;
    }

    public void setCalcPontoDeMass3D(boolean calcPontoDeMass3D) {
        this.calcPontoDeMass3D = calcPontoDeMass3D;
    }

    public boolean isCalcPontoDeMassMembros() {
        return calcPontoDeMassMembros;
    }

    public void setCalcPontoDeMassMembros(boolean calcPontoDeMassMembros) {
        this.calcPontoDeMassMembros = calcPontoDeMassMembros;
    }

    public int getSkelProfile() {
        return skelProfile;
    }

    public void setSkelProfile(int skelProfile) {
        this.skelProfile = skelProfile;
    }
}
